//
// Created by az on 2020/6/20.
//
#include <eigen3/Eigen/Dense>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>

#include <ros/ros.h>
#include <gazebo_msgs/GetModelState.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

#include "oriTrans.h"

ros::Publisher pos_pub_, pcd_obj_pub_, pcd_ground_pub_;
ros::ServiceClient client_;
gazebo_msgs::GetModelState objstate_;

bool hasPos_ = false;

void pointCloudPub();

oriTrans orientationTrans_;
nav_msgs::Odometry state_;
Eigen::Quaterniond quan_robot_;
double robot_yaw_ = 0;
void posPublish(){
    client_.call(objstate_);

    if (!objstate_.response.success) {
        ROS_ERROR("cant get robot state !");
        ros::Duration(3).sleep();
        return;
    }

    state_.header.frame_id = "wmr";
    state_.header.stamp    = ros::Time::now();

    state_.pose.pose       = objstate_.response.pose;
    state_.twist.twist     = objstate_.response.twist;
    quan_robot_.w() = state_.pose.pose.orientation.w;
    quan_robot_.x() = state_.pose.pose.orientation.x;
    quan_robot_.y() = state_.pose.pose.orientation.y;
    quan_robot_.z() = state_.pose.pose.orientation.z;

    Eigen::Matrix3d DCM;
    Eigen::Vector3d Eular;
    orientationTrans_.Quater2DCM(quan_robot_, &DCM);
    orientationTrans_.DCM2Euler(DCM, &Eular);
    //当前机器人的信息
    robot_yaw_ = Eular(2);

    pos_pub_.publish(state_);
    hasPos_ = true;
}

sensor_msgs::LaserScan laser_msg_;
void rcvLaserCallback(const sensor_msgs::LaserScan &msg){
    laser_msg_.header = msg.header;

    laser_msg_.angle_min       = msg.angle_min;
    laser_msg_.angle_max       = msg.angle_max;
    laser_msg_.angle_increment = msg.angle_increment;
    laser_msg_.range_min       = msg.range_min;
    laser_msg_.range_max       = msg.range_max;

    laser_msg_.ranges.clear();
    laser_msg_.ranges = msg.ranges;

    if(hasPos_){
        pointCloudPub();
    }
}

void pointCloudPub(){
    int x = state_.pose.pose.position.x;
    int y = state_.pose.pose.position.y;

    Eigen::Vector3d rob_pt(x, y, robot_yaw_);

    sensor_msgs::PointCloud2 occ_map, gro_map;
    pcl::PointCloud<pcl::PointXYZ> obj, ground;
    pcl::PointXYZ occ_pt, gro_pt;

    double angle = laser_msg_.angle_min;
    for(int i = 0; i < (int)laser_msg_.ranges.size(); i++){
        angle += laser_msg_.angle_increment;
        if(!std::isinf(laser_msg_.ranges[i])){
            double d = laser_msg_.ranges[i];
            if(d > laser_msg_.range_min && d <= laser_msg_.range_max){
                occ_pt.x = (float)(d * cos(angle));
                occ_pt.y = (float)(d * sin(angle));
                occ_pt.z = 0.1;
                obj.points.push_back(occ_pt);
            }
        } else {
            double d = laser_msg_.range_max;

            gro_pt.x = (float)(d * cos(angle));
            gro_pt.y = (float)(d * sin(angle));
            gro_pt.z = 0.1;
            ground.points.push_back(gro_pt);
        }
    }

    obj.width = obj.points.size();
    obj.height = 1;
    obj.is_dense = true;
    ground.width = ground.points.size();
    ground.height = 1;
    ground.is_dense = true;

    pcl::toROSMsg(obj, occ_map);
    pcl::toROSMsg(ground, gro_map);
    occ_map.header.frame_id = "odom";
    gro_map.header.frame_id = "odom";
    pcd_obj_pub_.publish(occ_map);
    pcd_ground_pub_.publish(gro_map);
}

int main(int argc, char **argv){
    ros::init(argc, argv, "pos_generator");
    ros::NodeHandle n("~");

    pos_pub_ = n.advertise<nav_msgs::Odometry>("/pos", 1);
    client_  = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");

    pcd_obj_pub_    = n.advertise<sensor_msgs::PointCloud2>("/obj_pcd", 5);
    pcd_ground_pub_ = n.advertise<sensor_msgs::PointCloud2>("/obj_ground", 5);

    ros::Subscriber laser_sub = n.subscribe("/scan",  1, rcvLaserCallback);

    objstate_.request.model_name = "turtlebot3_burger";
    //wait for the client fo gazebo service beginning
    client_.call(objstate_);
    while(!objstate_.response.success){
        ROS_WARN("wait for the gazebo service client !");
        ros::Duration(3).sleep();
        client_.call(objstate_);
        if (objstate_.response.success) {
            ROS_WARN("open the gazebo service client success !");
            break;
        }
    }
    ros::Rate rate(60);
    while(ros::ok()){
        posPublish();
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}